#include <L3G4200D.h>
#include <Wire.h>

L3G4200D::L3G4200D() 
{
  Wire.begin();
}

void L3G4200D::begin()
{
  setupL3G4200D(200);
}

void L3G4200D::end()
{

//RETIRAR so para ter codigo
  setupL3G4200D(200);
}


int L3G4200D::getX()
{
  byte xMSB = readRegister(L3G4200D_Address, 0x29);
  byte xLSB = readRegister(L3G4200D_Address, 0x28);
  return ((xMSB << 8) | xLSB);
}

int L3G4200D::getY()
{
  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
  return ((yMSB << 8) | yLSB);
}

int L3G4200D::getZ()
{
  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  return ((zMSB << 8) | zLSB);
}

int L3G4200D::setupL3G4200D(int scale)
{
  //From  Jim Lindblom of Sparkfun's code

  // Enable x, y, z and turn off power down:
  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

  // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

  // Configure CTRL_REG3 to generate data ready interrupt on INT2
  // No interrupts used on INT1, if you'd like to configure INT1
  // or INT2 otherwise, consult the datasheet:
  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

  // CTRL_REG4 controls the full-scale range, among other things:

  if(scale == 250)
    {
      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
    }
  else if(scale == 500)
    {
      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
    }
  else
    {
      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
    }

  // CTRL_REG5 controls high-pass filtering of outputs, use it
  // if you'd like:
  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}

void L3G4200D::writeRegister(int deviceAddress, byte address, byte val) 
{
    Wire.beginTransmission(deviceAddress); // start transmission to device 
    Wire.send(address);       // send register address
    Wire.send(val);         // send value to write
    Wire.endTransmission();     // end transmission
}

int L3G4200D::readRegister(int deviceAddress, byte address)
{
    int v;
    Wire.beginTransmission(deviceAddress);
    Wire.send(address); // register to read
    Wire.endTransmission();

    Wire.requestFrom(deviceAddress, 1); // read a byte

    while(!Wire.available()) 
	{
        // waiting
    }

    v = Wire.receive();
    return v;
}